package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Kinect;
import edu.wpi.first.wpilibj.KinectStick;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;

public class Main extends SimpleRobot{

    CANJaguar fr,fl,br,bl,pickup,topShoot,lowShoot;
    Servo shootYaw,shootPitch,dropper;
    public void robotInit(){
        fr=Drive.createJag(RobotMap.JAGUAR_FRONT_RIGHT);
        fl=Drive.createJag(RobotMap.JAGUAR_FRONT_LEFT);
        br=Drive.createJag(RobotMap.JAGUAR_BACK_RIGHT);
        bl=Drive.createJag(RobotMap.JAGUAR_BACK_LEFT);
        pickup=Drive.createJag(RobotMap.JAGUAR_PICKUP);
        topShoot=Drive.createJag(RobotMap.JAGUAR_SHOOTER_TOP);
        lowShoot=Drive.createJag(RobotMap.JAGUAR_SHOOTER_BOTTOM);

        dropper=new Servo(RobotMap.SERVO_DROPPER);
        shootYaw=new Servo(RobotMap.SERVO_YAW);
        shootPitch=new Servo(RobotMap.SERVO_PITCH);
    }
    public void autonomous(){
        Kinect kin=Kinect.getInstance();
        KinectStick leftStick=new KinectStick(RobotMap.KINECT_LEFT_ARM);
        KinectStick rightStick=new KinectStick(RobotMap.KINECT_RIGHT_ARM);
        double power=0.5,lastArmSet=RobotMap.V_SERVO_DROPPER_DEF;
        boolean inverse=false;
        while(isEnabled()&&isAutonomous()){
            //POWER SET
            power=0.5;
            if(leftStick.getRawButton(1))power=0.25;
            if(leftStick.getRawButton(2))power=1;
            //PICKUP TOGGLE
            Drive.setJag(pickup,leftStick.getRawButton(RobotMap.KINECT_LEFT_LEG_OUT)?.5:0);
            //DROPPER TOGGLE
            dropper.set(leftStick.getRawButton(RobotMap.KINECT_RIGHT_LEG_OUT)?RobotMap.V_SERVO_DROPPER_DOWN:RobotMap.V_SERVO_DROPPER_DEF);
            //TANK DRIVE
            Drive.setJag(fl,leftStick.getY());
            Drive.setJag(fr,-rightStick.getY());
            Drive.setJag(bl,Drive.readJag(fl));
            Drive.setJag(br,Drive.readJag(fr));

            Timer.delay(.05);
        }
    }
    public void operatorControl(){
        Joystick driver=new Joystick(1),shooter=new Joystick(2);
        boolean[][][] bools=new boolean[2][12][2];
        boolean inverse=false;
        double power=0.5,lastArmSet=RobotMap.V_SERVO_DROPPER_DEF;
        while(isEnabled()&&isOperatorControl()){
            //POWER SET
            power=0.5;
            if(driver.getRawButton(7))power=0.25;
            if(driver.getRawButton(8))power=1;
            //PICKUP TOGGLE
            Drive.setJag(pickup,Drive.toggleTest(driver,1,RobotMap.DUALACTION_LEFT_BUMPER,bools)?-.5:0);
            Drive.setJag(pickup,Drive.toggleTest(driver,1,RobotMap.DUALACTION_RIGHT_BUMPER,bools)?.5:0);
            //DROPPER TOGGLE
            Drive.manualDrop(driver,dropper,lastArmSet);
            //TANK DRIVE
            Drive.manualDrive(driver,fl,fr,bl,br,power,inverse);

            Timer.delay(.05);
        }
    }
}